function writePosition(N,P,G)

%This function writes the dynamics file for the double pendulum.

nStateStr = num2str(2*N);

filename = ['position_' num2str(N) '_link'];

comments{1} = ['[px,py,gx,gy] = ' upper(filename) '(Z,P)'];
comments{2} = ' ';
comments{3} = 'FUNCTION:  This function computes the cartesian positions';
comments{4} = '    of the CoM and tip of each link';
comments{5}  = 'INPUTS: ';
comments{10} = ['    z = [' nStateStr ' X nTime]  matrix of states'];
comments{11} = '    P = struct of parameters';
comments{8}  = 'OUTPUTS: ';
comments{9}  = '    px = [nLink X nTime] position of the end of each link';
comments{10} = '    py = [nLink X nTime] position of the end of each link';
comments{11} = '    gx = [nLink X nTime] position of the CoM of each link';
comments{12} = '    gy = [nLink X nTime] position of the CoM of each link';
comments{13} = ' ';
comments{14} = 'NOTES:';
comments{15} = ['    This file was automatically generated by ' mfilename '.m']; 

params{1} = {'g ','gravity'};
params{2} = {'m','mass'};
params{3} = {'l','length'};
params{4} = {'I','moment of inertia about its center of mass'};
params{5} = {'d','distance between center of mass and parent joint'};

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                               write file                                %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
fid = fopen([filename '.m'],'w');

fprintf(fid, ['function [px, py, gx, gy] = ' filename '(z,P) \n']);

for i=1:length(comments)
    fprintf(fid,['%%' comments{i} '\n']);
end
fprintf(fid,'\n');

for i=1:length(params)
    if i==1  %Gravity, scalar
    fprintf(fid,[params{i}{1} ' = P.' params{i}{1} '; %%' params{i}{2} '\n']);
    else  %unpack vectors:
        for j=1:N
           fprintf(fid,[params{i}{1} num2str(j) ...
               ' = P.' params{i}{1} '(' num2str(j) '); %% Link ' num2str(j) ' ' params{i}{2} '\n']);
        end        
    end
end
fprintf(fid,'\n');

for j = 1:N
   fprintf(fid,['th' num2str(j) ' = z(' num2str(j) ',:); \n']); 
end
fprintf(fid,'\n');

fprintf(fid,'nTime = length(th1); \n');

fprintf(fid,['px = zeros(' num2str(N) ',nTime);\n']);
fprintf(fid,['py = zeros(' num2str(N) ',nTime);\n']);
fprintf(fid,['gx = zeros(' num2str(N) ',nTime);\n']);
fprintf(fid,['gy = zeros(' num2str(N) ',nTime);\n']);
fprintf(fid,'\n');

for i=1:N
    fprintf(fid,['px(' num2str(i) ',:) = ' vectorize(char(P(i,1))) ';\n']);
    fprintf(fid,['py(' num2str(i) ',:) = ' vectorize(char(P(i,2))) ';\n']);
    fprintf(fid,['gx(' num2str(i) ',:) = ' vectorize(char(G(i,1))) ';\n']);
    fprintf(fid,['gy(' num2str(i) ',:) = ' vectorize(char(G(i,2))) ';\n']);
    fprintf(fid,'\n');
end

fprintf(fid,'\n');
fprintf(fid,'end \n');

fclose(fid);
end


